//
//  Kalman.h
//  TrackingSim
//
//  Created by Garrett Manka on 2/17/12.
//  Copyright (c) 2012. All rights reserved.
//

#ifndef TrackingSim_Kalman_h
#define TrackingSim_Kalman_h

#include <math/Matrix.h>

template < int N_States >
class KalmanFilter
{
public:
    
    KalmanFilter();
    
    void setState( const Matrix< N_States, 1 >& state );
    Matrix<N_States, 1> getState() const;
    
    
    void setCovariance( const Matrix<N_States,N_States>& cov );
    Matrix<N_States,N_States> getCovariance() const;
    
    template< int R_Z, int C_Z, int R_H, int C_H >
    void update( const Matrix<R_Z,C_Z>& y , const Matrix<R_H,C_H>& h , const Matrix<R_H,R_H>& R );
    
private:
    
    Matrix< N_States, 1 > m_state;
    Matrix< N_States , N_States > m_covariance;
};

template< int N_States >
KalmanFilter<N_States>::KalmanFilter()
{
    
}

template< int N_States>
void KalmanFilter<N_States>::setState(const Matrix<N_States, 1>& state)
{
    m_state = state;
}

template< int N_States >
Matrix<N_States, 1 > KalmanFilter<N_States>::getState() const
{
    return m_state;
}

template< int N_States>
void KalmanFilter<N_States>::setCovariance(const Matrix<N_States, N_States>& cov)
{
    m_covariance = cov;
}

template< int N_States>
Matrix<N_States,N_States> KalmanFilter<N_States>::getCovariance() const
{
    return m_covariance;
}

template< int N_States >
template< int R_Z, int C_Z, int R_H, int C_H >
void KalmanFilter<N_States>::update( const Matrix<R_Z,C_Z>& y , const Matrix<R_H,C_H>& h , const Matrix<R_H,R_H>& R )
{
    //Matrix<R_Z,C_Z> y = z - ( h * m_state );
    
    //Matrix<R_Z,C_Z> y = z - ( h * m_state );
    
    Matrix<R_H,R_H> S = h * m_covariance * h.tp() + R;
    
    Matrix<R_H,R_H> S_inv;
    
    invertMatrix(S, S_inv);
    
    Matrix<C_H,R_H> K = m_covariance * h.tp() * S_inv;
    
    m_state = m_state + K * y;
    
    m_covariance = m_covariance - K * h * m_covariance;
}

#endif
